
// 2. test Eigen::Quaterniond
//build: g++ -I/usr/include/eigen3 2.Eigen_Quaterniond.cpp 
#include <iostream>
#include <Eigen/Dense>
using namespace std;
int main(int argc, char** argv )
{
    Eigen::Quaterniond a(1,0,0,0);
    Eigen::Quaterniond b(1,0,0,0);
    Eigen::Quaterniond retval;
    if (a.x() * b.x() + a.y() * b.y() + a.z() * b.z() + a.w() * b.w() < 0.0) {
        b.x() = -b.x();
        b.y() = -b.y();
        b.z() = -b.z();
        b.w() = -b.w();
    }
    retval.x() = a.x() + b.x();
    retval.y() = a.y() + b.y();
    retval.z() = a.z() + b.z();
    retval.w() = a.w() + b.w();
    
    cout<<retval.w()<<endl;
    retval.normalize();
    cout<<retval.w()<<endl;
    // The next two lines are equal
    cout<<retval.matrix()<<endl;
    cout<<retval.toRotationMatrix()<<endl;


}
